FISST-SLAM: Finite Set Statistical Approach to Simultaneous Localization and Mapping

The solution to the problem of mapping an environment and at the same time using this map to localize (the simultaneous localization and mapping, SLAM, problem) is a key prerequisite in the synthesis of truly autonomous vehicles. By far the most common formulation of the SLAM problem is founded on a vector based stochastic framework, where the sensor models and the vehicle models are represented in state-space form and the joint posterior or its statistics are obtained based on recursive Bayesian estimation. All of these SLAM solutions leading from the stochastic vector state-space approach require that we solve certain parallel problems in each recursion. These include effective solutions to the problems of data association, feature extraction, clutter filtering, and landmark or map management. In this paper, we propose an alternative framework based on finite set statistics (FISST), where the SLAM problem is reformulated so that the landmark map and the measurements are represented using random finite sets and the landmark map is jointly estimated with the vehicle state vector, whilst explicitly accounting for measurement detection uncertainty, data-association uncertainty, false alarms and map management in the SLAM filter framework. Similar to FastSLAM, the proposed formulation is based on a factorization of the full SLAM posterior into a product of the vehicle trajectory posterior and the landmark map posterior conditioned on the vehicle trajectory. The vehicle trajectory posterior is then estimated using a particle filter and the map posterior conditioned on the vehicle trajectory via a sequential Monte Carlo (SMC) implementation of the probability hypothesis density (PHD) filter. Simulation results of the proposed algorithm are presented and benchmarked against FastSLAM to demonstrate The International Journal of Robotics Research Vol. 00, No. 00, Xxxxxxxx 2009, pp. 000–000 DOI: 10.1177/0278364909349948 c The Author(s), 2009. Reprints and permissions: http://www.sagepub.co.uk/journalsPermissions.nav Figures 2–6 appear in color online: http://ijr.sagepub.com the effectiveness and improved performance of the FISST-SLAM in the presence of significant clutter. KEY WORDS—SLAM, random finite sets, probability hypothesis density filter